#include "box_fitting.h"
#include <opencv2/opencv.hpp>
#include <limits>

static bool fitBoundingBox(
    const pcl::PointCloud<pcl::PointXYZI>& cluster,
    pcl::PointCloud<pcl::PointXYZI>& bbox)
{
    if (cluster.empty()) return false;
    std::vector<cv::Point2f> pts2d;
    float minZ = std::numeric_limits<float>::max();
    float maxZ = -std::numeric_limits<float>::max();

    for (const auto& pt : cluster) {
        pts2d.emplace_back(pt.x, pt.y);
        if (pt.z < minZ) minZ = pt.z;
        if (pt.z > maxZ) maxZ = pt.z;
    }
    if (pts2d.size() < 4) return false;
    cv::RotatedRect rect = cv::minAreaRect(pts2d);
    cv::Point2f rectPts[4];
    rect.points(rectPts);

    bbox.clear();
    for (int h = 0; h < 2; ++h) {
        float z = (h == 0) ? minZ : maxZ;
        for (int k = 0; k < 4; ++k) {
            bbox.push_back(pcl::PointXYZI{rectPts[k].x, rectPts[k].y, z, 0});
        }
    }
    return true;
}

void boxFitting(
    const std::vector<pcl::PointCloud<pcl::PointXYZI>>& objects,
    std::vector<pcl::PointCloud<pcl::PointXYZI>>& bbPoints)
{
    bbPoints.clear();
    for (const auto& cluster : objects) {
        pcl::PointCloud<pcl::PointXYZI> bbox;
        if (fitBoundingBox(cluster, bbox)) {
            bbPoints.push_back(bbox);
        }
    }
}